// ============================================================
// -----------------------PROYECTO FINAL----------------------- 
// ============================================================
// Universidad Del Valle De Guatemala.
// Algoritmos y Estructuras de Datos.
// Sección 20.
// Miguel Enrique Zea Arenales.
// Carné 09238.
// 20/11/2012.
// ------------------------------------------------------------
// Descripción:
// El sketch detallado a continuación determina la matriz de 
// adyacencia y aplica el algoritmo de Floyd sobre 2 capas de 
// información proveniente de un módulo GPS LEA-6S, luego de 
// ser procesada por el módulo de formato.
// ============================================================

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// LIBRERÍAS EMPLEADAS
// ------------------------------------------------------------
// - StackList.h -> implementación de una stack con listas 
//   encadenadas.
// - SD.h -> librería FAT16 para el manejo de archivos de una 
//   memoria SD.
// - stdlib.h -> librería de funciones estándar para 
//   compiladores C bajo norma ANSI.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <StackList.h> 
#include <SD.h>
#include <stdlib.h>

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// DEFINICIONES
// ------------------------------------------------------------
// - NO_CONNECTION -> magnitud considerada como una NO conexión 
//   entre 2 nodos de un grafo.
// - MAX_NODES -> número máximo de nodos a procesar, 7 para el 
//   ejemplo presentado, puede modificarse para incluir más 
//   (tomando en consideración los límites de RAM del Arduino).
// - LAYERS -> número de capas de información de GPS procesada,
//   2 pra el ejemplo presentado, puede aumentarse.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#define NO_CONNECTION 9999.00
#define MAX_NODES 7
#define LAYERS 2

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// DECLARACIÓN DE VARIABLES DE IMPORTANCIA
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
File GPSfile; // Objeto archivo empleado para la lectura y creación de archivos CSV en la memoria SD.
char faux[6]; // String auxiliar empleada para la extracción de datos de los archivos GPS.
char readChar; // Auxiliar empleado para la lectura de caracteres desde un archivo CSV.
boolean storeData = false, storeLat = false, storeLon = false, firstLocation = false; // Banderas empleadas para la extracción de información de los archivos GPS.
float data[MAX_NODES * 10], Nodes[MAX_NODES]; // Arreglos de floats empleados para almacenar la matriz de adyacencia y la ubicación de los nodos existentes.
float *adjMatrix = &data[0]; // Puntero para la matriz de adyacencia.
int path[MAX_NODES * 10]; // Arreglo de enteros empleado para almacenar la matriz de rutas construida por el algoritmo de Floyd.
int *pathMatrix = &path[0]; // Puntero para la matriz de rutas.
float cLat, cLon, pLat, pLon, apxDist; // Latitudes y longitudes actuales y pasadas para el cálculo de distancia entre puntos, al extraer la información de los archivos.
int charIndex, pIndex, cIndex, N; // Indices empleados para la extracción de datos y variable global que contiene el número de Nodos existentes.

// ============================================================
// INICIALIZACIÓN DE MATRIZ DE ADYACENCIA
// ------------------------------------------------------------
// PRECONDICIONES: debe haberse asignado un espacio de memoria
// lo suficientemente grande a la matriz apuntada por el puntero
// M. Debe haberse definido un número máximo de nodos.
// POSTCONDICIONES: se inicializa la matriz de adyacencia del
// grafo, colocando un cero en la diagonal y una no conexión
// en las casillas restantes.
// ============================================================
void initAdjMatrix(float *M, int n) {
  for(int i = 0; i < n; i++) {
    for(int j = 0; j < n; j++) {
      if(i == j) {
        *(M+i*n+j) = 0;
      } else {
        *(M+i*n+j) = NO_CONNECTION;  
      }
    }  
  }
}
// ============================================================

// ============================================================
// IMPRESIÓN PARA MATRICES PUNTO FLOTANTE
// ------------------------------------------------------------
// PRECONDICIONES: las dimensiones de la matriz a imprimir 
// deben coincidir con el parámetro n (se recomienda emplear
// el número máximo de nodos).
// POSTCONDICIONES: se imprime la matriz de floats con 
// tabulaciones y nuevas líneas entre sus elementos.
// ============================================================
void printFMatrix(float *A, int n) {
  int i, j;
  
  for(i = 0; i < n; i++) {
    for(j = 0; j < n; j++) {
      Serial.print(*(A+i*n+j));
      Serial.print('\t');
    }  
    Serial.println();
  }
  Serial.println();  
}
// ============================================================

// ============================================================
// IMPRESIÓN PARA MATRICES ENTEROS
// ------------------------------------------------------------
// PRECONDICIONES: las dimensiones de la matriz a imprimir 
// deben coincidir con el parámetro n (se recomienda emplear
// el número máximo de nodos).
// POSTCONDICIONES: se imprime la matriz de enteros con 
// tabulaciones y nuevas líneas entre sus elementos.
// ============================================================
void printIMatrix(int *A, int n) {
  int i, j;
  
  for(i = 0; i < n; i++) {
    for(j = 0; j < n; j++) {
      Serial.print(*(A+i*n+j));
      Serial.print('\t');
    }  
    Serial.println();
  }
  Serial.println();  
}
// ============================================================

// ============================================================
// APROXIMACIÓN DE DISTANCIA ENTRE DOS PUNTOS
// ------------------------------------------------------------
// PRECONDICIONES: ninguna.
// POSTCONDICIONES: se calcula la distancia entre dos puntos 
// como la magnitud del vector resultante de la resta entre las
// latitudes y longitudes de los puntos.
// ============================================================
float aproxDistance(float x1, float y1, float x2, float y2) {
  float x, y;
  x = x2 - x1;
  y = y2 - y1;
  return sqrt((x*x) + (y*y));  
}
// ============================================================

// ============================================================
// EXTRACCIÓN DE DATOS DE ARCHIVOS GPS
// ------------------------------------------------------------
// PRECONDICIONES: debe haberse inicializado previamente la
// matriz de adyacencia. Debe existir por lo menos un archivo
// de nombre "GPSLOG0X.CSV" en la memoria SD. Los archivos de
// información deben haber sido previamente formateados por el
// módulo respectivo.
// POSTCONDICIONES: se extrae la información contenida en cada
// uno de los archivos obtenidos del GPS. Se calcula un 
// aproximado de la distancia entre 2 puntos y se coloca en la
// casilla correspondiente de la matriz de adyacencia.
// ============================================================
void extractData(char logNo) {
  char generic[] = "GPSLOG0X.CSV"; // Nombre genérico de los archivos GPS.
  generic[7] = 0x30 + logNo; // Se establece el número de la capa a extraer información.
  
  // Se abre el archivo de información GPS en modo lectura.
  GPSfile = SD.open(generic, FILE_READ);
  if (GPSfile) {
    Serial.print(generic);
    Serial.println(" contents:");
     
    while (GPSfile.available()) {
      readChar = GPSfile.read(); // Se extrae la información caracter por caracter.
      
      // Si el caracter es un punto, se inicia el almacenamiento de caracteres en el string auxiliar.
      // Si el caracter es una coma o un carriage return, se termina el almacenamiento de datos y se convierte el string auxiliar en un float correspondiente ya sea a la
      // latitud o longitud.
      // Al haber extraído la latitud y longitud de una línea, se evalúa si el punto ya existe. Si no existe, se crea y se procede a calcular la distancia entre éste y
      // el nodo previo.
      if(storeData) {
          if((readChar == ',') || (readChar == 13)){
            storeData = false;
            if(storeLat) {
              cLat = atof(faux);
              faux[4] = 0x30;
              faux[5] = 0x00;
            } else if(storeLon) {
              cLon = atof(faux);
              faux[4] = 0x30;
              faux[5] = 0x00;
              
              if(firstLocation) {
                firstLocation = false;
                pLat = cLat;
                pLon = cLon;
                
                pIndex = -1;
                apxDist = aproxDistance(0, 0, cLat, cLon);
                for(int i = 0; i < N; i++) {
                  if(((apxDist - Nodes[i]) < 12) && ((apxDist - Nodes[i]) > -12)) {
                    pIndex = i;
                    break;
                  }
                }
                if(pIndex < 0) {
                  Nodes[N] = aproxDistance(0, 0, cLat, cLon);
                  pIndex = N;
                  N++; 
                }
                    
              } else {
                cIndex = -1;
                apxDist = aproxDistance(0, 0, cLat, cLon);
                for(int i = 0; i < N; i++) {
                  if(((apxDist - Nodes[i]) < 12) && ((apxDist - Nodes[i]) > -12)) {
                    cIndex = i;
                    break;
                  }
                }
                
                if(cIndex < 0) {
                  Nodes[N] = aproxDistance(0, 0, cLat, cLon);
                  cIndex = N;
                  N++; 
                }
            
                if(*(adjMatrix+pIndex*MAX_NODES+cIndex) > 9995) {
                  *(adjMatrix+pIndex*MAX_NODES+cIndex) = aproxDistance(pLat, pLon, cLat, cLon);  
                }
                pIndex = cIndex; 
                pLat = cLat;
                pLon = cLon;   
              }
            }  
          } else if(readChar == 10) {
          storeData = false;
          storeLat = false;
          storeLon = false;    
        } else {
          faux[charIndex] = readChar;
          charIndex++;
        } 
      } else if(readChar == '.') {
        charIndex = 0;
        storeData = true;
        if(!storeLat) {
          storeLat = true;
        } else {
          storeLat = false;
          storeLon = true; 
        }       
      } 
      
      Serial.write(readChar);
    }
    GPSfile.close();
  } else {
    Serial.println("error opening file");
  }
  
  firstLocation = true;  
  
}
// ============================================================

// ============================================================
// ALGORITMO DE FLOYD
// ------------------------------------------------------------
// PRECONDICIONES: el espcio en memoria designado a la matriz
// de adyacencia debe coincidir con el de la matriz de rutas y
// ambos con el número de nodos n (se recomienda emplear el
// número máximo de nodos). La matriz de rutas debe haberse 
// incializado previamente.
// POSTCONDICIONES: se aplica el algoritmo de Floyd sobre la
// matriz de adyacencia del grafo y se genera la matriz de
// rutas correspondiente, para la determinación de caminos más
// cortos.
// ============================================================
void Floyd(float *A, int *P, int n) {
  int i, j, k;
  
  for (k = 0; k < n; k++) {
    for (i = 0; i < n; i++) {
      for (j = 0; j < n; j++) { 
        if (*(A+i*n+k) + *(A+k*n+j) < *(A+i*n+j)) {
          *(A+i*n+j) = *(A+i*n+k)+ *(A+k*n+j);
          *(P+i*n+j) = *(P+k*n+j);
        }
      }
    }
  }
}
// ============================================================

// ============================================================
// INICIALIZACIÓN DE MATRIZ DE RUTAS
// ------------------------------------------------------------
// PRECONDICIONES: el espcio en memoria designado a la matriz
// de rutas debe coincidir con el de la matriz de adyacencia y
// con el número de nodos n (se recomienda emplear el número
// máximo de nodos). La matriz de adyacencia debe haberse
// inicializado previamente.
// POSTCONDICIONES: se genera una matriz de rutas inicial, 
// cuyas casillas son iguales al número de fila actual en caso
// de existir alguna conexión y -1 caso contrario.
// ============================================================
void initPathMatrix(float *A, int *P, int n) {
  int i, j;

  for(i = 0; i < n; i++) {
    for(j = 0; j < n; j++) {
      if(*(A+i*n+j) > NO_CONNECTION-5) {
        *(P+i*n+j) = -1;
      } else {
        *(P+i*n+j) = i;
      }
    }
  }
  
  for(i = 0; i < n; i++) {
    *(P+i*n+i) = i;
  }
}
// ============================================================

// ============================================================
// OBTENCIÓN DE RUTAS
// ------------------------------------------------------------
// PRECONDICIONES: debe haberse aplicado el algoritmo de Floyd
// sobre las matrices de adyacencia y rutas. Las dimensiones de
// la matriz de rutas deben coincidir con el número de nodos n
// (se recomienda emplear el número máximo de nodos). Los nodos
// de origen y destino deben de estar dentro de los límites
// establecidos por el número máximo de nodos.
// POSTCONDICIONES: se imprime el camino más corto entre los
// 2 nodos indicados.
// ============================================================
void getPath(int *P, int n, int origin, int destination) {
  int index;
  char *ptr;
  StackList<int> route;
  
  route.push(destination);
  while(*(pathMatrix+origin*n+destination) != origin) {
    route.push(*(pathMatrix+origin*n+destination));
    destination = *(pathMatrix+origin*n+destination);
  }
  route.push(origin);

  while(!route.isEmpty()) {
    Serial.print(route.pop());
    Serial.print(" ");
  }
  Serial.println();  
}
// ============================================================

// ============================================================
// CREACIÓN DE ARCHIVO CSV A PARTIR DE MATRIZ FLOATS
// ------------------------------------------------------------
// PRECONDICIONES: debe haberse aplicado el algoritmo de Floyd
// sobre las matrices de adyacencia y rutas. Las dimensiones de
// la matriz de adyacencia deben coincidir con el número de 
// nodos n (se recomienda emplear el número máximo de nodos). 
// El nombre del archivo debe corresponder a un 8.3 filename 
// con extensión CSV.
// POSTCONDICIONES: se crea un archivo con el nombre 
// especificado en la memoria SD, que contenga los valores de
// la matriz de adyacencia separados por comas.
// ============================================================
void createFCSV(float *M, int n, char *name) {
  
  if(SD.exists(name)) {
    SD.remove(name);  
  }
  
  GPSfile = SD.open(name, FILE_WRITE);
 
  if (GPSfile) {
    Serial.print("Writing to ");
    Serial.print(name);
    Serial.print("...");
    
    for(int i = 0; i < n; i++) {
      for(int j = 0; j < n; j++) {
        GPSfile.print(*(M+i*n+j));
        if(j == n - 1) {
        } else {
          GPSfile.print(',');    
        }
      }
      GPSfile.println();  
    }
    GPSfile.close();
    Serial.println("done.");
  } else {
    Serial.println("error opening file");
  }  
}
// ============================================================

// ============================================================
// CREACIÓN DE ARCHIVO CSV A PARTIR DE MATRIZ ENTEROS
// ------------------------------------------------------------
// PRECONDICIONES: debe haberse aplicado el algoritmo de Floyd
// sobre las matrices de adyacencia y rutas. Las dimensiones de
// la matriz de rutas deben coincidir con el número de nodos n
// (se recomienda emplear el número máximo de nodos). El nombre
// del archivo debe corresponder a un 8.3 filename con 
// extensión CSV.
// POSTCONDICIONES: se crea un archivo con el nombre 
// especificado en la memoria SD, que contenga los valores de
// la matriz de rutas separados por comas.
// ============================================================
void createICSV(int *M, int n, char *name) {
  
  if(SD.exists(name)) {
    SD.remove(name);  
  }
  
  GPSfile = SD.open(name, FILE_WRITE);
 
  if (GPSfile) {
    Serial.print("Writing to ");
    Serial.print(name);
    Serial.print("...");
    
    for(int i = 0; i < n; i++) {
      for(int j = 0; j < n; j++) {
        GPSfile.print(*(M+i*n+j));
        if(j == n - 1) {
        } else {
          GPSfile.print(',');    
        }
      }
      GPSfile.println();  
    }
    GPSfile.close();
    Serial.println("done.");
  } else {
    Serial.println("error opening file");
  }  
}
// ============================================================

// ============================================================
// CONFIGURACIÓN Y CÓDIGO ESTÁTICO
// ============================================================
void setup() {
  Serial.begin(9600); // Se inicializa el módulo de comunicación serial.
  
  N = 0; // Se inicializa el número de nodos actuales.
  faux[4] = 0x30; // Se inicializa el string auxiliar para la extracción de datos.
  faux[5] = 0x00;
  
  initAdjMatrix(adjMatrix, MAX_NODES); // Se inicializa la matriz de adyacencia del grafo de MAX_NODES nodos.
  
  // Se inicializa la memoria SD y se verifica su formato.
  Serial.print("Initializing SD card..."); 
  pinMode(10, OUTPUT);
 
  if (!SD.begin(10)) {
    Serial.println("initialization failed!"); // Se despliega un mensaje de error en caso de un formato incorrecto o si no hay ninguna memoria conectada.
    return;
  }
  Serial.println("initialization done."); // Se indica el formato correcto de la memoria.
 
  for(char k = 1; k <= LAYERS; k++) {
    extractData(k); // Se extrae la información de cada una de las capas de información obtenidas por el GPS.  
  }
  
  Serial.println("Initial Adjacency Matrix:");
  printFMatrix(adjMatrix, MAX_NODES); // Se imprime el estado inicial de la matriz de adyacencia.
  initPathMatrix(adjMatrix, pathMatrix, MAX_NODES); // Se incializa la matriz de rutas correspondiente a la matriz de adyacencia del grafo.
  Serial.println("Initial Path Matrix:");
  printIMatrix(pathMatrix, MAX_NODES); // Se imprime el estado inicial de la matriz de rutas.
  Floyd(adjMatrix, pathMatrix, MAX_NODES); // Se aplica el algoritmo de Floyd a las matrices de adyacencia y rutas asociadas al grafo.
  Serial.println("Adjacency Matrix after Floyd:");
  printFMatrix(adjMatrix, MAX_NODES); // Se imprime la matriz de adyacencia resultante.
  Serial.println("Path Matrix after Floyd:");
  printIMatrix(pathMatrix, MAX_NODES); // Se imprime la matriz de rutas resultante.
  Serial.println("Example - Shortest Path from Node 0 to Node 3:");
  getPath(pathMatrix, MAX_NODES, 0, 3); // Se genera el camino más corto entre el nodo 0 y el nodo 3 como ejemplo.
  createFCSV(adjMatrix, MAX_NODES, "ADJMAT.CSV"); // Se genera el archivo CSV de la matriz de adyacencia.
  createICSV(pathMatrix, MAX_NODES, "PATHMAT.CSV"); // Se genera el archivo CSV de la matriz de rutas.
}
// ============================================================

// ============================================================
// CICLO DE OPERACIÓN
// ============================================================
void loop() {
// No se realiza ninguna operación durante el ciclo infinito.
}
// ============================================================
